using System;
using System.Collections.Generic;
using System.Collections;
using System.Text;
using System.Drawing;
using System.Drawing.Imaging;
using Diversity.Robotics.Navigation;

namespace Diversity.Maths
{
    
    /// <summary>
    /// Defines the type of normalization to do.
    /// </summary>
    public enum Normalization {
        None,
        MaxToOne,
        IntegralToOne
    }

    /// <summary>
    /// Common probabilistic functions: normalization, gaussians and log odds.
    /// </summary>
    public static class ProbabilityUtils
    {
        private static Random _random;
        static ProbabilityUtils()
        {
            _random = new Random();
        }

        #region Gaussians

        /// <summary>
        /// Get samples across a Gaussian distribution
        /// </summary>
        /// <param name="center"></param>
        /// <param name="variance"></param>
        /// <param name="width"></param>
        /// <param name="numSamples"></param>
        /// <returns></returns>
        public static double[] CreateDistribution(double center, double variance, double width, int numSamples)
        {
            double[] distribution = new double[numSamples];
            double factor = 1.0 / Math.Sqrt(2 * Math.PI * variance);
            double start = center - (width/2);
            double step = width / numSamples;
            for (int i = 0; i < distribution.Length; i++)
            {
                distribution[i] = factor * Math.Pow(Math.E, -Math.Pow(start + (step* i) - center, 2) / (2 * variance));
            }
            return distribution;
        }

        /// <summary>
        /// Gets samples across half a Gaussian distribution
        /// </summary>
        /// <param name="centerAndStart"></param>
        /// <param name="variance"></param>
        /// <param name="width"></param>
        /// <param name="numSamples"></param>
        /// <returns></returns>
        public static double[] CreateHalfDistribution(double centerAndStart, double variance, double width, int numSamples)
        {
            double[] distribution = new double[numSamples];
            double factor = 1.0 / Math.Sqrt(2 * Math.PI * variance);
            
            double step = width / numSamples;
            for (int i = 0; i < distribution.Length; i++)
            {
                distribution[i] = factor * Math.Pow(Math.E, -Math.Pow(centerAndStart + (step * i) - centerAndStart, 2) / (2 * variance));
            }
            return distribution;
        }

        /// <summary>
        /// Gets a sample from a gaussian distribution
        /// </summary>
        /// <param name="variance"></param>
        /// <returns></returns>
        public static double RandomFromGaussian(double variance)
        {
            double deviation = Math.Sqrt(variance);
            double tot = 0;
            for (int i = 0; i < 12; i++)
            {
                tot += (_random.NextDouble() * deviation * 2) - deviation;
            }
            return tot / 2;
        }

        /// <summary>
        /// Calculates the probability of a given sample from within a Gaussian distribution
        /// </summary>
        /// <param name="val"></param>
        /// <param name="variance"></param>
        /// <returns></returns>
        public static double ProbabilityOfValInVariance(double val, double variance)
        {
            if (variance == 0)
            {
                return val == 0 ? 1 : 0;
            }
            else
            {
                return  (1.0 / Math.Sqrt(2 * Math.PI * variance)) * Math.Pow(Math.E, -((val * val) / (2 * variance)));
            }
        }


        public static double GetEffectiveSampleSize(double[] weights)
        {
            double tot = 0;
            foreach (double d in weights)
            {
                tot += Math.Pow(d, 2);
            }
            return 1 / (tot * weights.Length);
        }
        #endregion

        #region  ToString
        /// <summary>
        /// Dumps the values of the distribution into a string for debug purposes
        /// </summary>
        /// <param name="distribution"></param>
        /// <param name="formatString"></param>
        /// <returns></returns>
        public static string GetString(double[] distribution, string formatString)
        {
            StringBuilder sb = new StringBuilder();
            foreach (double d in distribution)
            {
                sb.Append(d.ToString(formatString) + " ");
            }
            return sb.ToString();
        }
        #endregion

        #region Normalization
        /// <summary>
        /// Normalizes a distribution
        /// </summary>
        /// <param name="distribution"></param>
        /// <param name="normalization"></param>
        /// <returns></returns>
        public static double[] Normalize(double[] distribution, Normalization normalization)
        {
            
           // double[] distribution2 = (double[]) distribution.Clone();
            switch (normalization)
            {
                case Normalization.None:
                    break;
                case Normalization.MaxToOne:
                    double max = GetMax(distribution);
                    if (max > 0)
                    {
                        for (int i = 0; i < distribution.Length; i++)
                        {
                            distribution[i] = distribution[i] / max;
                        }
                    } else
                    {
                        return distribution;
                    }
                    break;
                case Normalization.IntegralToOne:
                    double tot = GetTotal(distribution);
                    if (tot > 0)
                    {
                        for (int i = 0; i < distribution.Length; i++)
                        {
                            distribution[i] = distribution[i]/tot;
                        }
                    }
                    else {
                        return distribution; }
                    break;
            }
            return distribution;
        }

        /*
        public List<double> Normalize(List<double> distribution, Normalization normalization)
        {
            List<double> returnList = new List<double>();
           
            switch (normalization)
            {
                case Normalization.None:
                    returnList = distribution;
                    break;
                case Normalization.MaxToOne:
                    double max = double.MinValue;
                    distribution.ForEach(delegate(double d)
                         {
                             if (d > max) max = d;
                         });
                    distribution.ForEach(delegate(double d)
                       {
                           returnList.Add(d / max);
                       });
                    break;
                case Normalization.IntegralToOne:
                    double tot = 0;
                    distribution.ForEach(delegate(double d)
                        {
                            tot += d;
                        });
                    distribution.ForEach(delegate(double d)
                       {
                           returnList.Add(d / tot);
                       });
                    break;
            }
            return returnList;
        }
        */
        /*
        public double[,] Normalize(double[,] distribution, Normalization normalization)
        {
            double[,] distribution2 = (double[,])distribution.Clone();
            switch (normalization)
            {
                case Normalization.None:
                    break;
                case Normalization.MaxToOne:
                    double max = GetMax(distribution);
                    for (int x = 0; x < distribution.GetLength(0); x++)
                    {
                        for (int y = 0; y < distribution.GetLength(1); y++)
                        {
                            distribution2[x, y] = distribution[x, y] / max;
                        }
                    }
                    break;
                case Normalization.IntegralToOne:
                    double tot = GetTotal(distribution);
                    for (int x = 0; x < distribution.GetLength(0); x++)
                    {
                        for (int y = 0; y < distribution.GetLength(1); y++)
                        {
                            distribution2[x, y] = distribution[x, y] / tot;
                        }
                    }
                    break;
            }
            return distribution2;
        }
         * */
        #endregion

        #region LogOdds
        /// <summary>
        /// Converts a log odds representation into a fraction
        /// </summary>
        /// <param name="logOdds"></param>
        /// <returns></returns>
        public static  double LogOddsToProbability(double logOdds)
        {
           return 1.0 - (1.0 / (1 + Math.Pow(Math.E, logOdds)));
        }

        /// <summary>
        /// Converts a log odds distribution into fractions
        /// </summary>
        /// <param name="distribution"></param>
        /// <returns></returns>
        public static double[] LogOddsToProbability(double[] distribution)
        {
            double[] distribution2 = (double[])distribution.Clone();
            for (int i = 0; i < distribution2.Length; i++)
            {
                distribution2[i] = LogOddsToProbability(distribution[i]);
            }
            return distribution2;
        }

        /// <summary>
        /// Converts a 2D log odds distribution into fractions
        /// </summary>
        /// <param name="distribution"></param>
        /// <returns></returns>
        public static double[,] LogOddsToProbability(double[,] distribution)
        {
            double[,] distribution2 = (double[,])distribution.Clone();

            for (int x = 0; x < distribution2.GetLength(0); x++)
            {
                for (int y = 0; y < distribution2.GetLength(1); y++)
               {
                   distribution2[x, y] = LogOddsToProbability(distribution[x, y]);
               }
            }
            return distribution2;
        }

        /// <summary>
        /// Converts a fraction into its log odds representation
        /// </summary>
        /// <param name="probability"></param>
        /// <returns></returns>
        public static double ProbabilityToLogOdds(double probability)
        {
            return Math.Log(probability/(1-probability));
        }

        /// <summary>
        /// Converts a distribution of fractions into a log odds representation
        /// </summary>
        /// <param name="distribution"></param>
        /// <returns></returns>
        public static double[] ProbabilityToLogOdds(double[] distribution)
        {
            double[] distribution2 = (double[])distribution.Clone();
            for (int i = 0; i < distribution2.Length; i++)
            {
                distribution2[i] = ProbabilityToLogOdds(distribution[i]);
            }
            return distribution2;
        }

        /// <summary>
        /// Converts a 2D distribution of fractions into a log odds representation
        /// </summary>
        /// <param name="distribution"></param>
        /// <returns></returns>
        public static double[,] ProbabilityToLogOdds(double[,] distribution)
        {
            double[,] distribution2 = (double[,])distribution.Clone();
            for (int x = 0; x < distribution2.GetLength(0); x++)
            {
                for (int y = 0; y < distribution2.GetLength(1); y++)
                {
                    distribution2[x, y] = ProbabilityToLogOdds(distribution[x, y]);
                }
            }
            return distribution2;
        }
        #endregion

        #region Max Min Total
        /// <summary>
        /// Gets the max value of the distribution
        /// </summary>
        /// <param name="distribution"></param>
        /// <returns></returns>
        public static double GetMax(double[] distribution)
        {
            double max = double.MinValue;
            foreach (double d in distribution)
            {
                if (d > max)
                {
                    max = d;
                }
            }
            return max;
        }

        /// <summary>
        /// Gets the max value of a 2D distribution
        /// </summary>
        /// <param name="distribution"></param>
        /// <returns></returns>
        public static double GetMax(double[,] distribution)
        {
            double max = double.MinValue;
            for (int x = 0; x < distribution.GetLength(0); x++)
            {
                for (int y = 0; y < distribution.GetLength(1); y++)
                {
                    if (distribution[x, y] > max)
                    {
                        max = distribution[x, y];
                    }
                }
            }
            return max;
        }

        /// <summary>
        /// Gets the sum of the entire distribution
        /// </summary>
        /// <param name="distribution"></param>
        /// <returns></returns>
        public static double GetTotal(double[] distribution)
        {
            double tot = 0;
            foreach (double d in distribution)
            {
                tot += d;
            }
            return tot;
        }

        /// <summary>
        /// Gets the sum of a 2D distribution
        /// </summary>
        /// <param name="distribution"></param>
        /// <returns></returns>
        public static double GetTotal(double[,] distribution)
        {
            double tot = 0;
            for (int x = 0; x < distribution.GetLength(0); x++)
            {
                for (int y = 0; y < distribution.GetLength(1); y++)
                {
                    tot += distribution[x, y];
                }
            }
            return tot;
        }

        /// <summary>
        /// Returns the max, min and total of a distribution
        /// </summary>
        /// <param name="distribution"></param>
        /// <returns></returns>
        public static double[] GetMaxMinTotal(double[] distribution)
        {
            double max = double.MinValue;
            double min = double.MaxValue;
            double tot = 0;
            foreach (double d in distribution)
            {
                if (d > max)
                {
                    max = d;
                }
                if (d < min)
                {
                    min = d;
                }
                tot += d;
            }
            return new double[] {max,min,tot};
        }

        /// <summary>
        ///Gets the Max, Min and Total of a 2D distribution
        /// </summary>
        /// <param name="distribution"></param>
        /// <returns></returns>
        public static double[] GetMaxMinTotal(double[,] distribution)
        {
            double max = double.MinValue;
            double min = double.MaxValue;
            double tot = 0;
            for (int x = 0; x < distribution.GetLength(0); x++)
            {
                for (int y = 0; y < distribution.GetLength(1); y++)
                {
                    if (distribution[x, y] > max)
                    {
                        max = distribution[x, y];
                    }
                    if (distribution[x, y]< min)
                    {
                        min = distribution[x, y];
                    }
                    tot += distribution[x, y];
                }
            }
            return new double[] {max,min,tot};
        }
        #endregion

        #region Drawing

        /// <summary>
        /// Utility of drawing a distribution to an image
        /// </summary>
        /// <param name="image"></param>
        /// <param name="distribution"></param>
        /// <param name="drawStats"></param>
        /// <param name="col"></param>
        /// <param name="maxValue"></param>
        /// <returns></returns>
        public static Image DrawDistribution(Image image, double[] distribution, bool drawStats, Color col, double maxValue)
        {
            if (distribution.Length < 2)
            {
                return image;
            }
            // double[] distribution2 = Normalize(distribution, normalization);

            int xOffset = 10;
            int yOffset = 10;
            int drawWidth = image.Width - xOffset - xOffset;
            int drawHeight = image.Height - yOffset - yOffset - yOffset; // extra space for text
            float widthPerSample = (float)(drawWidth / (1.0 * distribution.Length));
            List<PointF> points = new List<PointF>();

            // find max,min,total
            double[] maxMinTot = GetMaxMinTotal(distribution);
            double max = maxMinTot[0];
            double min = maxMinTot[1];
            double tot = maxMinTot[2];

            if (max == 0)
            {
                return image;
            }
            // prepare the line
            for (int i = 0; i < distribution.Length; i++)
            {
                points.Add(
                    new PointF((float)(xOffset + widthPerSample * i),
                               (float)(image.Height - yOffset - yOffset - (drawHeight * Math.Min(maxValue,distribution[i]) / maxValue))));
            }
            Graphics g = Graphics.FromImage(image);
            // draw the line
            Pen pen = new Pen(col);
            g.DrawLines(pen, points.ToArray());
            // draw x axis
            g.DrawLine(Pens.Black, xOffset, image.Height - yOffset - yOffset, xOffset + drawWidth,
                       image.Height - yOffset - yOffset);
            // draw y axis
            g.DrawLine(Pens.Black, xOffset, image.Height - yOffset - yOffset, xOffset,
                       image.Height - yOffset - yOffset - drawHeight);
            if (drawStats)
            {
                g.DrawString(
                    "Samples/Max/Min/Total: " + distribution.Length + "/" + max.ToString("0.000") + "/" +
                    min.ToString("0.000") + "/" + tot.ToString("0.000"), new Font("Verdana", 7), Brushes.Black,
                    new PointF(xOffset, image.Height - yOffset - yOffset));
            }

            g.Save();
            pen.Dispose();
            g.Dispose();
            return image;
        }

        /// <summary>
        /// Utility for drawing a distribution to an image
        /// </summary>
        /// <param name="image"></param>
        /// <param name="distribution"></param>
        /// <param name="drawStats"></param>
        /// <param name="col"></param>
        /// <returns></returns>
        public static Image DrawDistribution(Image image, double[] distribution, bool drawStats, Color col)
        {
            if (distribution.Length < 2)
            {
                return image;
            }
            // double[] distribution2 = Normalize(distribution, normalization);

            int xOffset = 10;
            int yOffset = 10;
            int drawWidth = image.Width - xOffset - xOffset;
            int drawHeight = image.Height - yOffset - yOffset - yOffset; // extra space for text
            float widthPerSample = (float) (drawWidth/(1.0*distribution.Length));
            List<PointF> points = new List<PointF>();

            // find max,min,total
            double[] maxMinTot = GetMaxMinTotal(distribution);
            double max = maxMinTot[0];
            double min = maxMinTot[1];
            double tot = maxMinTot[2];

            if (max == 0) {
                return image;
            }
            // prepare the line
            for (int i = 0; i < distribution.Length; i++)
            {
                points.Add(
                    new PointF((float) (xOffset + widthPerSample*i),
                               (float) (image.Height - yOffset - yOffset - (drawHeight*distribution[i]/max))));
            }
            Graphics g = Graphics.FromImage(image);
            // draw the line
            Pen pen = new Pen(col);
            g.DrawLines(pen, points.ToArray());
            // draw x axis
            g.DrawLine(Pens.Black, xOffset, image.Height - yOffset - yOffset, xOffset + drawWidth,
                       image.Height - yOffset - yOffset);
            // draw y axis
            g.DrawLine(Pens.Black, xOffset, image.Height - yOffset - yOffset, xOffset,
                       image.Height - yOffset - yOffset - drawHeight);
            if (drawStats)
            {
                g.DrawString(
                    "Samples/Max/Min/Total: " + distribution.Length + "/" + max.ToString("0.000") + "/" +
                    min.ToString("0.000") + "/" + tot.ToString("0.000"), new Font("Verdana", 7), Brushes.Black,
                    new PointF(xOffset, image.Height - yOffset - yOffset));
            }

            g.Save();
            pen.Dispose();
            g.Dispose();
            return image;
        }

        /// <summary>
        /// Utility for drawing a distribution to an image after first sorting it.
        /// </summary>
        /// <param name="image"></param>
        /// <param name="distribution"></param>
        /// <param name="normalization"></param>
        /// <param name="col"></param>
        /// <param name="drawStats"></param>
        /// <returns></returns>
        public static Image DrawSortedDistribution(Image image, double[] distribution, Normalization normalization, Color col, bool drawStats)
        {
            if (distribution.Length == 0 || double.IsNaN(distribution[0]))
            {
                return image;
            }
            double[] distribution2 = Normalize(distribution, normalization);
            List<double> d2 = new List<double>();
            d2.AddRange(distribution2);
            d2.Sort();
            distribution2 = d2.ToArray();
          
            int xOffset = 10;
            int yOffset = 10;
            int drawWidth = image.Width - xOffset - xOffset;
            int drawHeight = image.Height - yOffset - yOffset - yOffset; // extra space for text
            float widthPerSample = (float)(drawWidth / (1.0 * distribution2.Length));
            List<PointF> points = new List<PointF>();

            // find max,min,total
            double[] maxMinTot = GetMaxMinTotal(distribution2);
            double max = maxMinTot[0];
            double min = maxMinTot[1];
            double tot = maxMinTot[2];

            if (double.IsNaN(distribution2[0]) || max == 0)
            {
                return image;
            }

            // prepare the line
            for (int i = 0; i < distribution2.Length; i++)
            {
                points.Add(new PointF((float)(xOffset + widthPerSample * i), (float)(image.Height - yOffset - yOffset - (drawHeight * distribution2[i] / max))));
            }

            Pen pen = new Pen(col);
            if (points.Count > 1)
            {
                Graphics g = Graphics.FromImage(image);
                // draw the line
                g.DrawLines(pen, points.ToArray());
                // draw x axis
                g.DrawLine(Pens.Black, xOffset, image.Height - yOffset - yOffset, xOffset + drawWidth,
                           image.Height - yOffset - yOffset);
                // draw y axis
                g.DrawLine(Pens.Black, xOffset, image.Height - yOffset - yOffset, xOffset,
                           image.Height - yOffset - yOffset - drawHeight);
                if (drawStats)
                {
                    g.DrawString(
                        "Samples/Max/Min/Total: " + distribution2.Length + "/" + max.ToString("0.000") + "/" +
                        min.ToString("0.000") + "/" + tot.ToString("0.000"), new Font("Verdana", 7), Brushes.Black,
                        new PointF(xOffset, image.Height - yOffset - yOffset));
                }
                pen.Dispose();
                g.Save();
                g.Dispose();
            }
            return image;
        }

        /// <summary>
        /// Draws a distribution to an image as a histogram
        /// </summary>
        /// <param name="image"></param>
        /// <param name="distribution"></param>
        /// <param name="normalization"></param>
        /// <returns></returns>
        public static Image DrawHistogram(Image image, double[] distribution, Normalization normalization)
        {
            if (image == null)
            {
                image = (Image)new Bitmap(distribution.GetLength(0), distribution.GetLength(1));
            }
            distribution = Normalize(distribution, normalization);

            int xOffset = 10;
            int yOffset = 10;
            int drawWidth = image.Width - xOffset - xOffset;
            int drawHeight = image.Height - yOffset - yOffset - yOffset; // extra space for text
            float widthPerSample = (float)(drawWidth / (1.0 * distribution.Length));
            List<PointF> points = new List<PointF>();

            // find max,min,total
            double[] maxMinTot = GetMaxMinTotal(distribution);
            double max = maxMinTot[0];
            double min = maxMinTot[1];
            double tot = maxMinTot[2];

            // prepare the line
            for (int i = 0; i < distribution.Length; i++)
            {
                points.Add(new PointF((float)Math.Max(xOffset,(xOffset -(widthPerSample/2) + widthPerSample * i)), (float)(image.Height - yOffset - yOffset - (drawHeight * distribution[i] / max))));
                points.Add(new PointF((float)Math.Min(xOffset+drawWidth,(xOffset + (widthPerSample/2) + widthPerSample * i)), (float)(image.Height - yOffset - yOffset - (drawHeight * distribution[i] / max))));
            }
            Graphics g = Graphics.FromImage(image);
            // draw the line
            g.DrawLines(Pens.Blue, points.ToArray());
            // draw x axis
            g.DrawLine(Pens.Black, xOffset, image.Height - yOffset - yOffset, xOffset + drawWidth, image.Height - yOffset - yOffset);
            // draw y axis
            g.DrawLine(Pens.Black, xOffset, image.Height - yOffset - yOffset, xOffset, image.Height - yOffset - yOffset - drawHeight);
            g.DrawString("Samples/Max/Min/Total: " + distribution.Length + "/" + max.ToString("0.000") + "/" + min.ToString("0.000") + "/" + tot.ToString("0.000"), new Font("Verdana", 7), Brushes.Black, new PointF(xOffset, image.Height - yOffset - yOffset));
            g.Save();
            g.Dispose();
            return image;
        }

        /// <summary>
        /// Draws a 2D distribution to an image
        /// </summary>
        /// <param name="image"></param>
        /// <param name="distribution"></param>
        /// <returns></returns>
        public static Image DrawDistribution(Image image, double[,] distribution)
         {
           // return  DrawDistribution(image, distribution, true, false);
           if (image == null)
             {
                 image = new Bitmap(distribution.GetLength(0), distribution.GetLength(1));
             }
           
            double max = GetMax(distribution);
       
             Bitmap b = (Bitmap)image;
             BitmapData data = b.LockBits(new Rectangle(0, 0, Math.Min(distribution.GetLength(0), b.Width), Math.Min(b.Height, distribution.GetLength(1))), ImageLockMode.WriteOnly, PixelFormat.Format24bppRgb);
             unsafe
             {
                 byte* imgPtr = (byte*)(data.Scan0);
                 for (int y = 0; y < data.Height; y++)
                 {
                     for (int x = 0; x < data.Width; x++)
                     {
                         byte val;
                         if (distribution[x, y] > 0)
                         {
                             val = (byte)(255 - Math.Round(distribution[x, y] * 255.0 / max));
                             imgPtr[0] = imgPtr[1] = imgPtr[2] = val;
                         }
                         else
                         {
                           imgPtr[0] = imgPtr[1] = imgPtr[2] = 255;
                         }
                         imgPtr += 3;
                     }
                     imgPtr += data.Stride - data.Width * 3;
                 }
             }
             b.UnlockBits(data);
             return b;
         }


        public static void DrawParticles(ref Bitmap b, Pose2D[] states, double scale)
        {
            // there must be a faster way, this isn't efficient

              double[,] poses = new double[b.Width, b.Height];
                    foreach (Pose2D state in states)
                    {
                        int x = (int) Math.Round(state.X/scale);
                        int y = (int) Math.Round(state.Y/scale);
                        if (x > 0 && y > 0 && x < b.Width && y < b.Height)
                        {
                            poses[x, y] = 1;
                        }
                    }


            BitmapData data = b.LockBits(new Rectangle(0, 0, b.Width, b.Height), ImageLockMode.ReadWrite, PixelFormat.Format24bppRgb);
            unsafe
            {
                byte* imgPtr = (byte*)(data.Scan0);
              
                for (int y = 0; y < data.Height; y++)
                {
                    for (int x = 0; x < data.Width; x++)
                    {
                        // byte val;
                        if (poses[x, y] > 0)
                        {
                            imgPtr[0] = 0;
                            imgPtr[1] = 0;
                            imgPtr[2] = 255; //red
                        }
                        imgPtr += 3;
                    }
                    imgPtr += data.Stride - data.Width * 3;
                }
            }
            b.UnlockBits(data);
        }

        /// <summary>
        /// Draws a distribution of particles to an image where the distribution represents the location of the particles
        /// </summary>
        /// <param name="b"></param>
        /// <param name="distribution"></param>
        public static void DrawParticles(ref Bitmap b, double[,] distribution)
        {
            //if (image == null)
          //  {
           //     image = new Bitmap(distribution.GetLength(0), distribution.GetLength(1));
          //  }
            // double[,] distribution2 = Normalize(distribution, normalization);

            // find max,min,total
            // double[] maxMinTot = GetMaxMinTotal(distribution); 
           // double max = GetMax(distribution);
            //double min = maxMinTot[1];
            // double tot = maxMinTot[2];
            //  Console.Out.WriteLine("Max/Min/Tot " + max + "/" + min + "/" + tot);
           // Bitmap b = (Bitmap)image;
            BitmapData data = b.LockBits(new Rectangle(0, 0, Math.Min(distribution.GetLength(0), b.Width), Math.Min(b.Height, distribution.GetLength(1))), ImageLockMode.ReadWrite, PixelFormat.Format24bppRgb);
            unsafe
            {
                byte* imgPtr = (byte*)(data.Scan0);
                for (int y = 0; y < data.Height; y++)
                {
                    for (int x = 0; x < data.Width; x++)
                    {
                       // byte val;
                        if (distribution[x, y] > 0)
                        {
                           // val = (byte)(255 - Math.Round(distribution[x, y] * 255.0 / max));
                           
                               // Console.Out.WriteLine("updating " + x + "," + y);
                                imgPtr[0] = 0;
                                imgPtr[1] = 0;
                                imgPtr[2] = 255; //red
                           
                           
                           // imgPtr[0] = imgPtr[1] = imgPtr[2] = val;
                        }
                        else
                        {
                           // if (imgPtr[0] > 0)
                           // {
                               // Console.Out.WriteLine("was " + imgPtr[0]);
                           // }
                          //imgPtr[0] = imgPtr[1] = imgPtr[2] = 255;
                               
                            //  imgPtr[0] = imgPtr[1] = imgPtr[2] = val;
                        }

                        imgPtr += 3;
                    }
                    imgPtr += data.Stride - data.Width * 3;
                }
            }
            b.UnlockBits(data);
           // return b;

        }

        /// <summary>
        /// Draws a 2D distribution of log odds to an image
        /// </summary>
        /// <param name="image"></param>
        /// <param name="distribution"></param>
        /// <returns></returns>
        public static Image DrawLogOddsDistribution(Image image, double[,] distribution)
        {
            if (image == null)
            {
                image = new Bitmap(distribution.GetLength(0), distribution.GetLength(1));
            }

            Bitmap b = (Bitmap)image;
            BitmapData data = b.LockBits(new Rectangle(0, 0, Math.Min(distribution.GetLength(0),b.Width), Math.Min(distribution.GetLength(1),b.Height)), ImageLockMode.WriteOnly, PixelFormat.Format24bppRgb);
            unsafe
            {
                byte* imgPtr = (byte*)(data.Scan0);
                for (int y = 0; y < data.Height; y++)
                {
                    for (int x = 0; x < data.Width; x++)
                    {
                        byte val;
                        //if (distribution[x, y] > 0)
                        //{
                            val = (byte)(255 - Math.Round( LogOddsToProbability(distribution[x, y]) * 255.0 ));
                        //}
                        //else
                        //{
                         //   val = 255;
                        //}
                        imgPtr[0] = imgPtr[1] = imgPtr[2] = val;
                        imgPtr += 3;
                    }
                    imgPtr += data.Stride - data.Width * 3;
                }
            }
            b.UnlockBits(data);
            return b;

        }

        /// <summary>
        /// Draws a 2D distribution of log odds to an image within an rectangle
        /// </summary>
        /// <param name="image"></param>
        /// <param name="distribution"></param>
        /// <param name="updateRect"></param>
        /// <returns></returns>
        public static Image DrawLogOddsDistribution(Image image, double[,] distribution, Rectangle updateRect)
        {
            if (image == null)
            {
                image = new Bitmap(distribution.GetLength(0), distribution.GetLength(1));
            }
           // double[,] distribution2 = Normalize(distribution, normalization);

            // find max,min,total
          //  double[] maxMinTot = GetMaxMinTotal(distribution2);
           // double max = GetMax(distribution);
           // double min = maxMinTot[1];
           //  double tot = maxMinTot[2];

            Bitmap b = (Bitmap)image;
            int left = Math.Max(updateRect.Left, 0);
            int top = Math.Max(updateRect.Top, 0);
            int width = Math.Min(updateRect.Width, distribution.GetLength(0) - left);
            int height = Math.Min(updateRect.Height, distribution.GetLength(1) - top);
            Rectangle imgRect = new Rectangle(left,top,width,height);
            BitmapData data = b.LockBits(imgRect, ImageLockMode.WriteOnly, PixelFormat.Format24bppRgb);
            unsafe
            {
                byte* imgPtr = (byte*)(data.Scan0);
                for (int y = 0; y < data.Height; y++)
                {
                    for (int x = 0; x < data.Width; x++)
                    {
                        byte val;
                        //if (distribution[left+x, top+y] > 0)
                      //  {
                            val = (byte)(255 - Math.Round(LogOddsToProbability(distribution[x, y]) * 255.0));
                       // }
                       // else
                       // {
                        //    val = 255;
                       // }
                        imgPtr[0] = imgPtr[1] = imgPtr[2] = val;
                        imgPtr += 3;
                    }
                    imgPtr += data.Stride - data.Width * 3;
                }
            }
            b.UnlockBits(data);
            return b;

        }
        #endregion

        /*
        public DifferentialDriveState[] ReSample(DifferentialDriveState[] states, double[] weights)
        {
            int numSamples = weights.Length;
            List<DifferentialDriveState> ReturnSamples = new List<DifferentialDriveState>();
            double r = _random.NextDouble() / numSamples;
            double c = weights[0];
            int i = 0;
            double U;

            for (int m = 0; m < numSamples; m++)
            {
                
                if (m>0)
                {
                    U = r + (m*1.0) / numSamples;   
                } else
                {
                    U = r;
                }
                while (U > c)
                {
                    i++;
                    if (i >= numSamples - 1)
                    {
                        // protect from overflow and return to start
                        Console.Out.WriteLine("back");
                        i = 0;
                    }
                    c = c + weights[i];
                    if (c == 0)
                    {
                        Console.Out.WriteLine("Problem: zero weight particle");
                        return ReturnSamples.ToArray();
                    }
                }
                ReturnSamples.Add(states[i]);
            }
            return ReturnSamples.ToArray();
        }
        */
        /*
        public List<object[]> ReSample(DifferentialDriveState[] states, Map[] maps, double[] weights)
        {
            int numSamples = weights.Length;
            List<object[]> ReturnSamples = new List<object[]>();
            List<DifferentialDriveState> returnStates = new List<DifferentialDriveState>();
            List<Map> returnMaps = new List<Map>();

            double r = _random.NextDouble() / numSamples;
            double c = weights[0];
            int i = 0;

            for (int m = 0; m < numSamples; m++)
            {
                double U = r + (m - 1.0) / numSamples;
                
                while (U > c)
                {
                    i++;
                    if (i >= numSamples - 1)
                    {
                        // protect from overflow and return to start
                        Console.Out.WriteLine("back");
                        i = 0;
                    }
                    c = c + weights[i];
                    if (c == 0)
                    {
                        Console.Out.WriteLine("Problem");
                        return ReturnSamples;
                    }
                }
                returnStates.Add(states[i]);
                returnMaps.Add(maps[i]);
            }
            ReturnSamples.Add((object[])returnStates.ToArray());
            ReturnSamples.Add((object[])returnMaps.ToArray());
            return ReturnSamples;
        }
         * */

        /// <summary>
        /// Does 'low variance re-sampling'
        /// Expects a set where the integral is one
        /// </summary>
        /// <param name="weights">The weights of current particles</param>
        /// <returns>The indexes of particles in the new set</returns>
        public static int[] ReSample(double[] weights)
        {
            int[] returnSamples = new int[weights.Length];
            int s = 0;

            double r = _random.NextDouble() / weights.Length;
            double c = weights[0];
            int i = 0;

            for (int m = 0; m < weights.Length; m++)
            {
                double U = (m > 0) ? r + ((m * 1.0) / weights.Length) : r;
                while (U > c)
                {
                    i = (i >= weights.Length - 1) ? 0 : i + 1;
                    c = c + weights[i];
                }
                returnSamples[s++] = i;
            }
            return returnSamples;
        }

        /// <summary>
        /// Returns the mean of a set of states.
        /// </summary>
        /// <param name="states"></param>
        /// <returns></returns>
        public static Pose2D GetMeanState(Pose2D[] states)
        {
            double x = 0;
            double y = 0;
            double theta = 0;
            foreach (Pose2D state in states)
            {
                x += state.X;
                y += state.Y;
                theta += state.Theta;
            }
            x = x / states.Length;
            y = y / states.Length;
            theta = theta / states.Length;
            return new Pose2D(x, y, theta);
        }


        /// <summary>
        /// Returns the root of the mean square error between a perfect state and an array of imperfect states.
        /// </summary>
        /// <param name="states"></param>
        /// <param name="perfectState"></param>
        /// <returns></returns>
        public static double GetRMSError(Pose2D[] states, Pose2D perfectState)
        {
           // DDState meanState = GetMeanState(states);
            
            double d = 0;
            foreach (Pose2D state in states)
            {
                d += Math.Pow(state.DistanceTo(perfectState),2);
            }
            d = d / states.Length;
           // Console.Out.WriteLine("Mean " + meanState.ToString());
           // Console.Out.WriteLine("RMSE: " + d);
            return d;
        }

        public static double[] BayesUpdate(double[] prior, double[] data)
        {
            double sum = GetTotal(data) + GetTotal(prior);
            if (sum > 0)
            {
                for (int i = 0; i < prior.Length; i++)
                {
                    prior[i] = (prior[i]*data[i])/sum;
                }

                return Normalize(prior, Normalization.IntegralToOne);
            } 
            else
            {
                return prior;
            }
        }

        #region Matrix Utilities

        /// <summary>
        /// Re-writes rows as columns
        /// </summary>
        /// <param name="matrix"></param>
        /// <returns></returns>
        public static double[,] MatrixTranspose(double[,] matrix)
        {
            int numRows = matrix.GetLength(0);
            int numColumns = matrix.GetLength(1);

            double[,] returnMatrix = new double[numColumns,numRows];
            for(int r=0; r< numRows; r++)
            {
                for (int c =0; c<numColumns; c++)
                {
                    returnMatrix[c, r] = matrix[r, c];
                }
            }
            return returnMatrix;
        }

        /// <summary>
        /// Multiplies two matrixes together
        /// </summary>
        /// <param name="m1"></param>
        /// <param name="m2"></param>
        /// <returns></returns>
        public static double[,] MatrixMuliply(double[,] m1, double[,] m2)
        {
            int m1Rows = m1.GetLength(0);
            int m1Cols = m1.GetLength(1);
            int m2Rows = m2.GetLength(0);
            int m2Cols = m2.GetLength(1);

            double[,] returnMatrix = new double[m1Rows,m2Cols];
            for(int r=0; r<m1Rows; r++)
            {
                for (int r2= 0; r2<m2Cols; r2++)
                {
                    for (int c = 0; c < m2Rows; c++)
                    {
                        returnMatrix[r, r2] += m1[r, c]*m2[c, r2];
                    }
                }
            }
            return returnMatrix;
        }

        public static double[,] MatrixMuliply(double[,] m1, double[,] m2, double[,] m3)
        {
            return MatrixMuliply(m1,MatrixMuliply(m2, m3));
        }

        public static double[,] MatrixMuliply2(double[,] m1, double[,] m2, double[,] m3)
        {
            return MatrixMuliply(MatrixMuliply(m1, m2),m3);
        }

        /// <summary>
        /// Writes a matrix as a string for debug purposes
        /// </summary>
        /// <param name="matrix"></param>
        public static void MatrixToString(double[,] matrix)
        {

            for(int r = 0; r< matrix.GetLength(0); r++)
            {
                string s = "";
                for(int c= 0; c< matrix.GetLength(1); c++)
                {
                    s += matrix[r, c] + "\t\t";
                }
                Console.Out.WriteLine(s);
            }
        }
        #endregion
    }
}
